/*
 * Path.cpp
 *
 *  Created on: Oct 24, 2010
 *      Author: sanjeev
 */

#include "PathSrv.h"

PathSrv::PathSrv(){

}
void PathSrv::initialize(fnavfn::Path::Request &req) {
	printed = false;
	w = req.width;
	h = req.height;
	grain = req.resolution;
	goalx = req.goalx;
	goaly = req.goaly;

	pathx = req.pathy;
	pathy = req.pathx;

	ROS_INFO("Width %d Height %d Resolution %lf PathLength %d",w,h,grain,pathx.size());

	gdist.clear();
	obs.clear();

	int cnt = 0;
	VECTOR::iterator beg, end, itr;
	for( itr = req.gdist.begin(), beg = req.gdist.begin(); itr != req.gdist.end(); itr++, cnt++ )
		if( cnt == w ){
			gdist.push_back(VECTOR(beg,itr));
			cnt = 0;
			beg = itr;
		}
	gdist.push_back(VECTOR(beg,itr));

	cnt = 0;
	for( itr = req.obs.begin(), beg = req.obs.begin(); itr != req.obs.end(); itr++, cnt++ )
		if( cnt == w ){
			obs.push_back(VECTOR(beg,itr));
			cnt = 0;
			beg = itr;
		}
	obs.push_back(VECTOR(beg,itr));

	/** sanity check **/
	assert((int)obs.size() == h );
	assert((int)gdist.size() == h );
	assert((int)obs[0].size() == w );
	assert((int)gdist[0].size() == w );
}

double PathSrv::getObstacleCostAt(int x,int y){
	/**
	if(!printed){
		printed = true;
		for(int i=0;i<h;i++){
			for(int j=0;j<w;j++)
				printf("%lf ",obs[i][j]);
			printf("\n");
		}
	}
	**/
	return obs[x][y];
}

PathSrv::~PathSrv() {
	// TODO Auto-generated destructor stub
}
